#include "ndt_match.h"
#include <ros/ros.h>
#include <iostream>
#include <iterator>
#include <cstdlib>
#include <csignal>

Eigen::Matrix4f init_guess_Get(const int &id)
{
  // ROS_INFO("录入初始变换矩阵");
  Eigen::Matrix4f init_guess = Eigen::Matrix4f::Identity();
  // 设置初始变换矩阵
  if (id == 1)
  {
    // ROS_INFO("b2a车初始变换矩阵");
    init_guess << 1, 0, 0, 0,
        0, 1, 0, -0.6,
        0, 0, 1, 0,
        0, 0, 0, 0;
  }
  else if (id == 2)
  {
    // ROS_INFO("c2a车初始变换矩阵");
    init_guess << 1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1, 0.6,
        0, 0, 0, 0;
  }
  else if (id == 3)
  {
    // ROS_INFO("c2b车初始变换矩阵");
    init_guess << 1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1, 0,
        0, 0, 0, 0;
  }
  else
  {
    ROS_ERROR("未知车辆");
  }

  return init_guess;
}

// 对Eigen::Matrix4f进行手动调整，去除俯仰角和翻滚角，以及TZ
void adjustTransformation(Eigen::Matrix4f &transform_matrix)
{
  // 提取旋转部分的矩阵
  Eigen::Matrix3f rotation_matrix = transform_matrix.block<3, 3>(0, 0);

  // 将俯仰和翻滚角设置为 0
  Eigen::Vector3f euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // Yaw (Z), Pitch (Y), Roll (X)

// 将俯仰和翻滚角设置为 0
    euler_angles[1] = 0.0f; // Pitch
    euler_angles[2] = 0.0f; // Roll
 // 使用修改后的欧拉角重新计算旋转矩阵
    Eigen::Matrix3f new_rotation_matrix;
    new_rotation_matrix = Eigen::AngleAxisf(euler_angles[0], Eigen::Vector3f::UnitZ()) * // Yaw
                          Eigen::AngleAxisf(euler_angles[1], Eigen::Vector3f::UnitY()) * // Pitch (now 0)
                          Eigen::AngleAxisf(euler_angles[2], Eigen::Vector3f::UnitX());  // Roll (now 0)
  // 更新旋转部分的矩阵
  transform_matrix.block<3, 3>(0, 0) = rotation_matrix;

  // 将平移向量的 Z 分量设置为 0
  transform_matrix(2, 3) = 0.0f;
}

Eigen::Matrix4f NDTMatching_M(const pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud_source,
                              const pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud_target, const Eigen::Matrix4f init_guess)
{
  if (cloud_source->empty() || cloud_target->empty())
  {
    std::cerr << "输入点云为空" << std::endl;
    return init_guess;
  }

  pcl::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI> ndt;
  ndt.setTransformationEpsilon(1e-12);  // 为终止条件设置最小转换差异
  ndt.setEuclideanFitnessEpsilon(1e-5); // 设置收敛条件
  ndt.setStepSize(1);                   // 为more-thuente线搜索设置最大步长
  ndt.setResolution(1.0);               // 设置NDT网格网格结构的分辨率（voxelgridcovariance）
  ndt.setMaximumIterations(5000);       // 添加最大迭代次数限制能够增加程序的鲁棒性阻止了它在错误的方向上运行时间过长
  ndt.setInputSource(cloud_source);     // 源点云
  ndt.setInputTarget(cloud_target);     // 目标点云

  pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZI>);
  ndt.align(*output_cloud, init_guess);
  // Eigen::Matrix4f NDT_result = ndt.getFinalTransformation();
  if (ndt.hasConverged())
  {
    // std::cout << "NDT配准成功!" << std::endl;
    std::cout << "Fitness Score: " << ndt.getFitnessScore() << std::endl;
    // 返回最终的转换矩阵
    // ROS_INFO("ndt.getFinalTransformation(): %f", ndt.getFinalTransformation()(0, 3));
    return ndt.getFinalTransformation();
  }
  else
  {
    // std::cerr << "NDT配准失败" << std::endl;
    // 如果未收敛，返回初始猜测
    return init_guess;
  }
}
